#include "axbopenglwidget.h"
#include "octree.h"
#include "function.h"
#include "mytimer.h"
#include <QKeyEvent>
#include <thread>

QPoint deltaPos;
const float PI=3.1415926;
float fov=45.0;
float yaw=-90;
float pitch=0;
unsigned int VBO[3], VAO[3];
std::vector<OctreeNode*> waynode;
std::vector<OctreeNode*> collisionnode;   //发生碰撞的八叉树节点
std::vector<OctreeNode*> collisionnode_2;
std::vector<OctreeNode*> collisionnode_3;
std::vector<OctreeNode*> collisionnode_4;
std::unordered_set<Point*> results;       //发生碰撞的点
std::unordered_set<Point*> results_2;
std::unordered_set<Point*> results_3;
std::unordered_set<Point*> results_4;
std::vector<OctreeNode*> scannode;        //扫描区域内必须被更新的八叉树节点的集合
std::vector<OctreeNode*> rayintersectnode;//模拟激光扫描点
//std::unordered_set<std::tuple<Point*,OctreeNode*>,decltype(tuple_hash),decltype(tuple_equal)> scanpoint;
std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal> scanpoint;
int count_for_collisionpoint = 0;
//std::unordered_set<Point,decltype(myPointHash)> hashpoint(1,myPointHash);
//单位立方体
float vertices[36][3] = {
{-0.5f, -0.5f, -0.5f},
 {0.5f, -0.5f, -0.5f},
 {0.5f, 0.5f, -0.5f},
 {0.5f, 0.5f, -0.5f},
{-0.5f, 0.5f, -0.5f},
{-0.5f, -0.5f, -0.5f},

{-0.5f, -0.5f, 0.5f},
{0.5f, -0.5f, 0.5f},
{0.5f, 0.5f, 0.5f},
{0.5f, 0.5f, 0.5f},
{-0.5f, 0.5f, 0.5f},
{-0.5f, -0.5f, 0.5f},

{-0.5f, 0.5f, 0.5f},
{-0.5f, 0.5f, -0.5f},
{-0.5f, -0.5f, -0.5f},
{-0.5f, -0.5f, -0.5f},
{-0.5f, -0.5f, 0.5f},
{-0.5f, 0.5f, 0.5f},

 {0.5f, 0.5f, 0.5f},
 {0.5f, 0.5f, -0.5f},
 {0.5f, -0.5f, -0.5f},
 {0.5f, -0.5f, -0.5f},
 {0.5f, -0.5f, 0.5f},
 {0.5f, 0.5f, 0.5f},

{-0.5f, -0.5f, -0.5f},
 {0.5f, -0.5f, -0.5f},
 {0.5f, -0.5f, 0.5f},
 {0.5f, -0.5f, 0.5f},
{-0.5f, -0.5f, 0.5f},
{-0.5f, -0.5f, -0.5f},

{-0.5f, 0.5f, -0.5f},
 {0.5f, 0.5f, -0.5f},
 {0.5f, 0.5f, 0.5f},
 {0.5f, 0.5f, 0.5f},
{-0.5f, 0.5f, 0.5f},
{-0.5f, 0.5f, -0.5f}
};

float point[3] = {0,0,0};
float line[2][3] ={
    {0,0,0},
    {1,0,0}
};
AXBOpenGLWidget::AXBOpenGLWidget(QWidget *parent) : QOpenGLWidget(parent)
{
    setFocusPolicy(Qt::StrongFocus);
    //定时重画
    connect(&timer,SIGNAL(timeout()),this,SLOT(on_timeout()));
    timer.start(TIMEOUTMSEC);

    m_viewmodel = ONE;

    Cuboid space;
    //获取三角函数值
    CELLTimestamp ttime;
    double TTIME = 0;
    ttime.update();
    getTrigonometricFunction(0.03);
    TTIME += ttime.getElapsedTimeInMilliSec();
    std::cout << "TTIME consumes time =  " << TTIME << std::endl;
    // 方便八叉树划分，空间坐标原点定于几何中心
    space.m_center = { 0.f, 0.f, 0.f };

    // 设置半长
    space.m_halflength = 50.f;
    space.m_halfwidth = 50.f;
    space.m_halfheight = 50.f;

    // 设置旋转轴，就是法向量
    space.m_axis[0] = Vector( 1.f, 0.f, 0.f );
    space.m_axis[1] = Vector( 0.f, 1.f, 0.f );
    space.m_axis[2] = Vector( 0.f, 0.f, 1.f );
    //建立一颗八叉树
    std::cout << "Loading octree...\n";
    CELLTimestamp TIME;
    float TIME_COUNT = 0.F;
    TIME.update();
    Octree* octree = new Octree();
    m_octree = octree;
    m_octree->BuildOctree(nullptr,1,space.m_center, space.m_halflength, space.m_halfwidth, space.m_halfheight, OCTREE_DEPTH);
    TIME_COUNT += TIME.getElapsedTimeInMilliSec();
    std::cout << "Build octree consumes time =  " << TIME_COUNT << std::endl;
    CELLTimestamp timer;
    float timer_answer = 0.0f;
#ifdef POINTCLOUD
   std::vector<std::vector<double>> pointdata = read_gt_file("C:/Users/17321/Desktop/csvfiles/pointcloud_1-SOLVED.csv");
   for (int i = 0; i < pointdata.size(); ++i)
   {
       //在插入阶段就可以抛弃一些点
        Point* tmp = new Point();
        tmp->m_x = pointdata[i][0];
        tmp->m_y = pointdata[i][1];
        tmp->m_z = pointdata[i][2]-1.5;
        //插入opengl中
        timer.update();
        m_octree->InsertObject(m_octree->GetRoot(),tmp);
        m_scenepoints.push_back(tmp);
        timer_answer += timer.getElapsedTimeInMilliSec();
   }
  std::cout <<" PointCloud.size() = " << pointdata.size() <<std::endl;
#endif //POINTCLOUD
   srand((unsigned)time(NULL));
#ifndef POINTCLOUD
//   for (int i = 0; i < POINTSNUMS; ++i)
//     {
//          Point* tmp = new Point();
//          int length = space.m_halflength * 200;
//          int width = space.m_halfwidth * 200;
//          int height = space.m_halfheight * 200;
//          tmp->m_x = static_cast<float>(rand() % length - space.m_halflength*100)/100;
//          tmp->m_y = static_cast<float>(rand() % width - space.m_halfwidth*100)/100;
//          tmp->m_z = static_cast<float>(rand() % height - space.m_halfheight*100)/100;
//          //插入opengl中
//          openglnode.push_back(tmp);
//          timer.update();
//          OctreeNode::InsertObjectToScene(octree, tmp);
//          timer_answer += timer.getElapsedTimeInMilliSec();
//     }
//     buildScene();
//    hashpoint.rehash(200000);
     //创造一些场景点
     for(int i = -40; i <= 40; i += 10 ){
         for(int j = -20; j <= 20; j += 20){
             Point center(j,i,-0.75);
             buildObject(center,5,2,1,0.15);
         }
     }
//     //创造一些无属性点
//     for (int i = 0; i < SCANNUMS; ++i)
//     {
//         //在插入阶段就可以抛弃一些点,需要自己判断一下z轴的中心坐标
//          Point* tmp = new Point();
//          int length = 8000;
//          int width = 8000;
//          tmp->m_x = static_cast<float>(rand() % length-length/2)/100;
//          tmp->m_y = static_cast<float>(rand() % width -width/2)/100;
//          tmp->m_z = -1;
//          buildObstacle(*tmp,0.5);
//          delete tmp;
//     }
#endif
   std::cout << "Insert consumes time = " << timer_answer << "ms" << std::endl;

    //产生机器人的三个部件
    Cuboid robot;
    Cuboid robot2;
    Cuboid robot3;
    Cuboid robot4;
    //激光传感器
    Cuboid laser;

    robot.m_center = { 0.f, 0.f, 0.f };

    // 设置机器人大小
    robot.m_halflength = 0.2f;
    robot.m_halfwidth = 0.6f;
    robot.m_halfheight = 0.375f;

    robot2.m_halflength = 0.75f;
    robot2.m_halfwidth = 0.15f;
    robot2.m_halfheight = 0.05f;

    robot3.m_halflength = 0.75f;
    robot3.m_halfwidth = 0.15f;
    robot3.m_halfheight = 0.05f;

    robot4.m_halflength = 0.2f;
    robot4.m_halfwidth = 0.6f;
    robot4.m_halfheight = 0.375f;
    // 设置机器人方向

    robot.m_R2 = pow(robot.m_halflength * robot.m_halflength + robot.m_halfwidth * robot.m_halfwidth,0.5);

    robot2.m_R2 = pow(robot2.m_halflength * robot2.m_halflength + robot2.m_halfwidth * robot2.m_halfwidth,0.5);

    robot3.m_R2 = pow(robot3.m_halflength * robot3.m_halflength + robot3.m_halfwidth * robot3.m_halfwidth,0.5);

    robot4.m_R2 = pow(robot4.m_halflength * robot4.m_halflength + robot4.m_halfwidth * robot4.m_halfwidth,0.5);


    //float maxdelta = pow(robot.m_halflength * robot.m_halflength + robot.m_halfwidth * robot.m_halfwidth, 0.5);
    //随机在空间中产生位置
    //robot.m_center.m_x = static_cast<float>(((rand() % (int)((space.m_halflength - maxdelta) * 200)) + maxdelta * 100 - space.m_halflength * 100)) / 100;//根据需要设置除数
    //robot.m_center.m_y = static_cast<float>(((rand() % (int)((space.m_halfwidth - maxdelta) * 200)) + maxdelta * 100 - space.m_halfwidth * 100)) / 100;
    robot.m_center.m_x = 0;
    robot.m_center.m_y = 0;
    robot4.m_center.m_x = robot.m_center.m_x;
    robot4.m_center.m_y = robot.m_center.m_y;

    robot.m_center.m_z = -1.125f;
    robot4.m_center.m_z = -0.375f;
    robot2.m_center.m_z = -1.45f;
    robot3.m_center.m_z = -1.45f;

    beziercurve.p0 = robot.m_center + Vector(10,45,-0.375);
    beziercurve.p1 = robot.m_center + Vector(10,0,-0.375);
    beziercurve.p2 = robot.m_center + Vector(-10,-20,-0.375);
    beziercurve.p3 = robot.m_center + Vector(-30,-45,-0.375);
    m_pos = 0;

    m_bzaccuracy = 1000;
    totalpath = 0;
    Vector dev = {beziercurve.dev(0).m_x,beziercurve.dev(0).m_y,beziercurve.dev(0).m_z};
    float rotation = atan(dev.m_y/dev.m_x);
    getPath(m_bzaccuracy);
    std::cout<<"Beziercurve's total length = "<<beziercurve.total_length()<<std::endl;

    //激光器的位置,目前先假设和部件一的中心同一个位置

    laser.m_center.m_z = robot.m_center.m_z;
    //激光器的扫描半径
    laser.m_R3 = LASERSCAN_R;


    //计算tan，用来绘制出合适的机器人
    float r = (robot.m_halfwidth  - robot2.m_halfwidth)/(robot.m_halflength + robot2.m_halflength);
    //计算角度
    float b = atan(r);
    m_atan = b;
    //计算两个中心点之间的模长
    float m = pow((robot.m_halfwidth  - robot2.m_halfwidth)*(robot.m_halfwidth  - robot2.m_halfwidth)+\
                  (robot.m_halflength + robot2.m_halflength)*(robot.m_halflength + robot2.m_halflength),0.5);
    m_len = m;
    //随机产生角度,部件零和x轴的夹角
    //float rotation = static_cast<float>(rand() % 3000) / 1000;
    m_rotation = rotation/PI*180;
    robot.setAxe(rotation);
    robot2.setAxe(rotation);
    robot3.setAxe(rotation);
    robot4.setAxe(rotation);

    robot2.m_center.m_x = robot.m_center.m_x + m*cos(b+rotation);
    robot2.m_center.m_y = robot.m_center.m_y + m*sin(b+rotation);
    robot3.m_center.m_x = robot.m_center.m_x + m*cos(-b+rotation);
    robot3.m_center.m_y = robot.m_center.m_y + m*sin(-b+rotation);

    laser.m_center.m_y = (robot2.m_center.m_y+robot3.m_center.m_y)/2;
    laser.m_center.m_x = (robot2.m_center.m_x+robot3.m_center.m_x)/2;
    lasers.push_back(laser);

    robot.setVert(rotation);
    robot2.setVert(rotation);
    robot3.setVert(rotation);
    robot4.setVert(rotation);
#ifdef DEBUG
    std::cout << "robot [x] = " << robot.m_center.m_x << std::endl;
    std::cout << "robot [y] = " << robot.m_center.m_y << std::endl;
    std::cout << "robot [x] = " << robot2.m_center.m_x << std::endl;
    std::cout << "robot [y] = " << robot2.m_center.m_y << std::endl;
    std::cout << "robot [x] = " << robot3.m_center.m_x << std::endl;
    std::cout << "robot [y] = " << robot3.m_center.m_y << std::endl;
#endif // DEBUG

    robotcenter.push_back(robot);
    robotcenter.push_back(robot2);
    robotcenter.push_back(robot3);
    robotcenter.push_back(robot4);
    //获得所有相交点
    //getRayIntersectPoints();
#ifdef DEBUG
    std::cout << "robot[rotation] = " << rotation << std::endl;
#endif // DEBUG


    //测试
    std::cout<<"\nSphere --------------------------------------------------------------------------Sphere"<<std::endl;
    m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeSphere,getCollisionPoint);
    m_CollisionStrategy->calculateToParts(m_octree->GetRoot(),robotcenter);
    m_CollisionStrategy->printResult();
    m_CollisionStrategy->clearResult();
    std::cout<<"Sphere --------------------------------------------------------------------------Sphere\n"<<std::endl;
    delete m_CollisionStrategy;
    //把结果集传递给opengl,让发生碰撞的改变颜色
    for (const auto& point : results)
    {
       openglnode.push_back(point);
    }

    //测试
    std::cout<<"\nAABB ------------------------------------------------------------------------------AABB"<<std::endl;
    m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeAABB,getCollisionPoint);
    m_CollisionStrategy->calculateToParts(m_octree->GetRoot(),robotcenter);
    m_CollisionStrategy->printResult();
    m_CollisionStrategy->clearResult();
    std::cout<<"AABB ------------------------------------------------------------------------------AABB\n"<<std::endl;
    delete m_CollisionStrategy;

    //测试
    std::cout<<"\nOBB3D ----------------------------------------------------------------------------OBB3D"<<std::endl;
    m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeOBB,getCollisionPoint);
    m_CollisionStrategy->calculateToParts(m_octree->GetRoot(),robotcenter);
    m_CollisionStrategy->printResult();
    m_CollisionStrategy->clearResult();
    std::cout<<"OBB3D ----------------------------------------------------------------------------OBB3D\n"<<std::endl;
    delete m_CollisionStrategy;

    //测试
    std::cout<<"\nOBB2D ----------------------------------------------------------------------------OBB2D"<<std::endl;
    m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeOBB2D,getCollisionPoint);
    m_CollisionStrategy->calculateToParts(m_octree->GetRoot(),robotcenter);
    m_CollisionStrategy->printResult();
    m_CollisionStrategy->clearResult();
    std::cout<<"OBB2D ----------------------------------------------------------------------------OBB2D\n"<<std::endl;
    delete m_CollisionStrategy;


//        m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeOBBThread,getCollisionPointThread);
//        std::thread calculate_1([this](){
//            m_CollisionStrategy->calculateToOnePart(m_octree->GetRoot(),robotcenter[0],collisionnode,results);
//            m_CollisionStrategy->mut.lock();
//            m_CollisionStrategy->printResult();
//            m_CollisionStrategy->clearResult();
//            m_CollisionStrategy->mut.unlock();
//        });
//        calculate_1.join();
//        std::thread calculate_4([this](){
//            m_CollisionStrategy->calculateToOnePart(m_octree->GetRoot(),robotcenter[3],collisionnode_4,results_4);
//            m_CollisionStrategy->mut.lock();
//            m_CollisionStrategy->printResult();
//            m_CollisionStrategy->clearResult();
//            m_CollisionStrategy->mut.unlock();
//        });
//        calculate_4.join();
//        std::thread calculate_2([this](){
//            m_CollisionStrategy->calculateToOnePart(m_octree->GetRoot(),robotcenter[1],collisionnode_2,results_2);
//            m_CollisionStrategy->mut.lock();
//            m_CollisionStrategy->printResult();
//            m_CollisionStrategy->clearResult();
//            m_CollisionStrategy->mut.unlock();
//        });
//        calculate_2.join();
//        std::thread calculate_3([this](){
//            m_CollisionStrategy->calculateToOnePart(m_octree->GetRoot(),robotcenter[2],collisionnode_3,results_3);
//            m_CollisionStrategy->mut.lock();
//            m_CollisionStrategy->printResult();
//            m_CollisionStrategy->clearResult();
//            m_CollisionStrategy->mut.unlock();
//        });
//        calculate_3.join();

//        delete m_CollisionStrategy;
    m_shape = AXBOpenGLWidget::ALL;
}

AXBOpenGLWidget::~AXBOpenGLWidget()
{
    //析构时释放对象
    makeCurrent();
    glDeleteBuffers(9,VBO);
    glDeleteVertexArrays(9,VAO);
    doneCurrent();

    delete m_octree;

    delete m_CollisionStrategy;
}

void AXBOpenGLWidget::drawShape(AXBOpenGLWidget::Shape shape)
{
    m_shape=shape;
    update();
}

//线框渲染还是点渲染
void AXBOpenGLWidget::setWirefame(bool wireframe)
{
    makeCurrent();
    if(wireframe)
        glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
    else
        glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
    update();
    doneCurrent();
}

void AXBOpenGLWidget::initializeGL()
{
    initializeOpenGLFunctions();

//-------------------------VAO,VBO相关------------------------------//
    //创建和绑定VBO和VAO对象，第一个参数表征创建的对象个数
    glGenVertexArrays(3, VAO);
    glGenBuffers(3, VBO);

    //绑定第一个VAO,VBO相关, 画三角形
    glBindVertexArray(VAO[0]);
    glBindBuffer(GL_ARRAY_BUFFER, VBO[0]);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    //告知显卡如何解析缓冲里的属性值，第一个属性指明其传递的数据送往顶点着色器location = 0的变量。
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);
    //解除VAO,VBO绑定
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    glBindVertexArray(0);

    //绑定第二个VAO,VBO相关， 画点
    glBindVertexArray(VAO[1]);
    glBindBuffer(GL_ARRAY_BUFFER, VBO[1]);
    glBufferData(GL_ARRAY_BUFFER,sizeof(point), point, GL_STATIC_DRAW);

    //告知显卡如何解析缓冲里的属性值，第一个属性指明其传递的数据送往顶点着色器location = 0的变量。
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);
    //解除VAO,VBO绑定
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    glBindVertexArray(0);

    //绑定第二个VAO,VBO相关， 画线
    glBindVertexArray(VAO[2]);
    glBindBuffer(GL_ARRAY_BUFFER, VBO[2]);
    glBufferData(GL_ARRAY_BUFFER,sizeof(line), line, GL_STATIC_DRAW);

    //告知显卡如何解析缓冲里的属性值，第一个属性指明其传递的数据送往顶点着色器location = 0的变量。
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), (void*)0);
    glEnableVertexAttribArray(0);
    //解除VAO,VBO绑定
    glBindBuffer(GL_ARRAY_BUFFER, 0);
    glBindVertexArray(0);
//-------------------------VAO,VBO相关------------------------------//


//-----------------------创建着色器程序相关---------------------------//
    bool success;
    shaderProgram.addShaderFromSourceFile(QOpenGLShader::Vertex,":/shaders/shapes.vert");
    shaderProgram.addShaderFromSourceFile(QOpenGLShader::Fragment,":/shaders/shapes.frag");
    success=shaderProgram.link();
    if(!success)
        qDebug()<<"ERR:"<<shaderProgram.log();
    shaderProgram.bind();
//-----------------------创建着色器程序相关---------------------------//
    //投影转换，实现透视投影
    QMatrix4x4 projection;
    projection.perspective(45,(float)width()/height(),0.1,VIEWDEPTH);
    shaderProgram.setUniformValue("projection", projection);
}

void AXBOpenGLWidget::resizeGL(int w, int h)
{
    Q_UNUSED(w);Q_UNUSED(h);
    //glViewport(0, 0, w, h);
}

void AXBOpenGLWidget::paintGL()
{
    openglnode.clear();
    //小车沿着贝塞尔曲线行驶
    //noCollisionPath();
    //测试,碰撞检测
    m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeOBB2D,getCollisionPoint);
    m_CollisionStrategy->calculateToParts(m_octree->GetRoot(),robotcenter);

    //m_CollisionStrategy->printResult();


    //把结果集传递给opengl,让发生碰撞的改变颜色
    for (const auto& point : results)
    {
       openglnode.push_back(point);
    }
    m_CollisionStrategy->clearResult();
        //delete m_CollisionStrategy;

    //背景绘制
    glClearColor(0.f, 0.f, 0.f, 1.0f);
    //深度缓冲，是否遮挡
    glEnable(GL_DEPTH_TEST);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glClear(GL_COLOR_BUFFER_BIT);
    //绑定着色器
    shaderProgram.bind();
    QMatrix4x4 view = setCamerView(m_viewmodel);
    shaderProgram.setUniformValue("view", view);
    //绘制坐标轴
    drawAxis();
    //绘制激光
    //drawRay();
    switch (m_shape) {
    case ALL:
        drawRayPoint();
        drawCollisionNode();
        drawAllNode();
        drawRobot();
        break;
    case COLLISION:
        drawCollisionNode();
        drawRobot();
    default:
        break;
    }
}

void AXBOpenGLWidget::keyPressEvent(QKeyEvent *event)
{
    float cameraSpeed=static_cast<float>(3*TIMEOUTMSEC/1000.0);

    if(m_viewmodel == THREE)
    {
        switch (event->key()) {
        case Qt::Key_F: m_viewmodel = ONE; break;
        case Qt::Key_Q: cameraPos += cameraSpeed * cameraFront;break;
        case Qt::Key_E: cameraPos -= cameraSpeed * cameraFront;break;
        case Qt::Key_D: cameraPos += cameraSpeed * cameraRight;break;
        case Qt::Key_A: cameraPos -= cameraSpeed * cameraRight;break;
        case Qt::Key_W: cameraPos += cameraSpeed * camerForword;break;
        case Qt::Key_S: cameraPos -= cameraSpeed * camerForword;break;
        default:
            break;
        }
    }
    else if(m_viewmodel == ONE)
    {
        switch (event->key()) {
        case Qt::Key_F:
           initViewThree();
           m_viewmodel = THREE;
            break;
        case Qt::Key_W:
            robotcenter[0].m_center.m_x = robotcenter[0].m_center.m_x + cameraSpeed*cos(m_rotation/180*PI);
            robotcenter[0].m_center.m_y = robotcenter[0].m_center.m_y + cameraSpeed*sin(m_rotation/180*PI);
            robotcenter[3].m_center.m_x = robotcenter[3].m_center.m_x + cameraSpeed*cos(m_rotation/180*PI);
            robotcenter[3].m_center.m_y = robotcenter[3].m_center.m_y + cameraSpeed*sin(m_rotation/180*PI);
            lasers[0].m_center.m_x = robotcenter[0].m_center.m_x;
            lasers[0].m_center.m_y = robotcenter[0].m_center.m_y;
            robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+m_rotation/180*PI);
            robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+m_rotation/180*PI);
#ifdef SCAN
            scan();
            //getRayIntersectPoints();
#endif
            break;
        case Qt::Key_S:
            robotcenter[0].m_center.m_x = robotcenter[0].m_center.m_x - cameraSpeed*cos(m_rotation/180*PI);
            robotcenter[0].m_center.m_y = robotcenter[0].m_center.m_y - cameraSpeed*sin(m_rotation/180*PI);
            robotcenter[3].m_center.m_x = robotcenter[3].m_center.m_x - cameraSpeed*cos(m_rotation/180*PI);
            robotcenter[3].m_center.m_y = robotcenter[3].m_center.m_y - cameraSpeed*sin(m_rotation/180*PI);
            lasers[0].m_center.m_x = robotcenter[0].m_center.m_x;
            lasers[0].m_center.m_y = robotcenter[0].m_center.m_y;
            robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+m_rotation/180*PI);
            robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+m_rotation/180*PI);
#ifdef SCAN
            scan();
            //getRayIntersectPoints();
#endif
            break;
        case Qt::Key_D:
            robotcenter[0].m_center.m_x = robotcenter[0].m_center.m_x + cameraSpeed*sin(m_rotation/180*PI);
            robotcenter[0].m_center.m_y = robotcenter[0].m_center.m_y - cameraSpeed*cos(m_rotation/180*PI);
            robotcenter[3].m_center.m_x = robotcenter[3].m_center.m_x + cameraSpeed*sin(m_rotation/180*PI);
            robotcenter[3].m_center.m_y = robotcenter[3].m_center.m_y - cameraSpeed*cos(m_rotation/180*PI);
            lasers[0].m_center.m_x = robotcenter[0].m_center.m_x;
            lasers[0].m_center.m_y = robotcenter[0].m_center.m_y;
            robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+m_rotation/180*PI);
            robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+m_rotation/180*PI);
#ifdef SCAN
            scan();
            //getRayIntersectPoints();
#endif
            break;
        case Qt::Key_A:
            robotcenter[0].m_center.m_x = robotcenter[0].m_center.m_x - cameraSpeed*sin(m_rotation/180*PI);
            robotcenter[0].m_center.m_y = robotcenter[0].m_center.m_y + cameraSpeed*cos(m_rotation/180*PI);
            robotcenter[3].m_center.m_x = robotcenter[3].m_center.m_x - cameraSpeed*sin(m_rotation/180*PI);
            robotcenter[3].m_center.m_y = robotcenter[3].m_center.m_y + cameraSpeed*cos(m_rotation/180*PI);
            lasers[0].m_center.m_x = robotcenter[0].m_center.m_x;
            lasers[0].m_center.m_y = robotcenter[0].m_center.m_y;
            robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+m_rotation/180*PI);
            robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+m_rotation/180*PI);
#ifdef SCAN
            scan();
            //getRayIntersectPoints();

#endif
            break;
        case Qt::Key_Q:
            m_rotation +=  2;
            robotcenter[0].setAxe(m_rotation/180*PI);
            robotcenter[3].setAxe(m_rotation/180*PI);
            robotcenter[1].setAxe(m_rotation/180*PI);
            robotcenter[2].setAxe(m_rotation/180*PI);
            robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+m_rotation/180*PI);
            robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+m_rotation/180*PI);
            //scan();
            break;
        case Qt::Key_E:
            m_rotation -=  2;
            robotcenter[0].setAxe(m_rotation/180*PI);
            robotcenter[3].setAxe(m_rotation/180*PI);
            robotcenter[1].setAxe(m_rotation/180*PI);
            robotcenter[2].setAxe(m_rotation/180*PI);
            robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+m_rotation/180*PI);
            robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+m_rotation/180*PI);
            robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+m_rotation/180*PI);
            //scan();
            break;
        default:
            break;
        }
    }

    shaderProgram.bind();
    update();
}

void AXBOpenGLWidget::mouseMoveEvent(QMouseEvent *event)
{
    auto currentPos=event->pos();
    deltaPos=currentPos-lastPos;
    lastPos=currentPos;
    float sensitivity = 0.1f; // change this value to your liking
    deltaPos *= sensitivity;
    yaw += deltaPos.x();
    pitch -= deltaPos.y();
    // reversed since y-coordinates go from bottom to top
    //qDebug()<<yaw<<","<<pitch;
    if(pitch > 89.0f) pitch = 89.0f;
    if(pitch < -89.0f) pitch = -89.0f;
    cameraFront.setX(cos(yaw*PI/180) * cos(pitch*PI/180));
    cameraFront.setY(sin(pitch*PI/180));
    cameraFront.setZ(sin(yaw*PI/180) * cos(pitch*PI/180));
    up = QVector3D(0.0f, -1.0f, 0.0f);
    cameraRight = QVector3D::crossProduct(up, cameraFront);
    camerForword = QVector3D::crossProduct(cameraRight,cameraFront);
    cameraRight.normalize();
    cameraFront.normalize();
    update();
}
void AXBOpenGLWidget::mousePressEvent(QMouseEvent *event)
{
    lastPos = event->pos();
}

void AXBOpenGLWidget::wheelEvent(QWheelEvent *event)
{
    if(fov >= 1.0f && fov <= 75.0f)
        fov -= event->angleDelta().y()/120;//一步是120
    if(fov <= 1.0f) fov = 1.0f;
    if(fov >= 75.0f) fov = 75.0f;
    update();
}


void AXBOpenGLWidget::on_timeout()
{
    update();
}
//清空UI界面中的所有点
void AXBOpenGLWidget::clearOctreeNode(){
    m_scenepoints.clear();     //场景点的集合
    m_obstaclepoints.clear();  //障碍物点的集合
    m_noattpoints.clear();      //无属性点的集合
}
//获得八叉树中的所有点,并将其插入对应的集合中
void AXBOpenGLWidget::getOctreeNode(OctreeNode* octree) {
    if (octree == nullptr)
        return;

    //到达叶子节点，写入结果集
    if(octree->m_children[0] == nullptr){
        Point* realhead = octree->m_scenepointslist;
        while (octree->m_scenepointslist != nullptr) {
            m_scenepoints.push_back(octree->m_scenepointslist);
            octree->m_scenepointslist = octree->m_scenepointslist->m_next;
        }
        octree->m_scenepointslist = realhead;

        Point* realhead_ob = octree->m_obstaclepointslist;
        while (octree->m_obstaclepointslist != nullptr) {
            m_obstaclepoints.push_back(octree->m_obstaclepointslist);
            octree->m_obstaclepointslist = octree->m_obstaclepointslist->m_next;
        }
        octree->m_obstaclepointslist = realhead_ob;

        for(auto& point : octree->m_noattributepointslist){
            m_noattpoints.push_back(point);
        }
    }
    else {
        for(int i = 0; i < 8; ++i)//不是叶子节点则递归调用
            getOctreeNode(octree->m_children[i]);
    }

}

void AXBOpenGLWidget::scan(){
    //CELLTimestamp time;
    //time.update();
    //float timer = 0.0;
    clearOctreeNode();
    //laserScanningByNeighbor(m_octree,lasers[0],scanpoint,0,PI);
    scanpoint.clear();
    double minsqDistance = 100000;
    std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal> scan_from_0_to_45 = {};
    std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal> scan_from_45_to_90 = {};
    std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal> scan_from_90_to_135 = {};
    std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal> scan_from_135_to_180 = {};
    std::atomic_bool flag_1 = false;
    std::atomic_bool flag_2 = false;
    std::atomic_bool flag_3 = false;
    std::atomic_bool flag_4 = false;
    std::thread scan_1([this,&scan_from_0_to_45,&flag_1](){
        laserScanningByNeighbor(m_octree,lasers[0],scan_from_0_to_45,0,PI/4);
        flag_1 = true;
    });
    std::thread scan_2([this,&scan_from_45_to_90,&flag_2](){
        laserScanningByNeighbor(m_octree,lasers[0],scan_from_45_to_90,PI/4,PI/2);
        flag_2 = true;
    });
    std::thread scan_3([this,&scan_from_90_to_135,&flag_3](){
        laserScanningByNeighbor(m_octree,lasers[0],scan_from_90_to_135,PI/2.0,PI*0.75);
        flag_3 = true;
    });
    std::thread scan_4([this,&scan_from_135_to_180,&flag_4](){
        laserScanningByNeighbor(m_octree,lasers[0],scan_from_135_to_180,PI*0.75,PI);
        flag_4 = true;
    });
    while(!flag_1 || !flag_2 || !flag_3 || !flag_4){
        //std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    for(auto itbegin = scan_from_0_to_45.begin(); itbegin != scan_from_0_to_45.end(); ++itbegin){
        scanpoint.insert(*itbegin);
    }
    for(auto itbegin = scan_from_45_to_90.begin(); itbegin != scan_from_45_to_90.end(); ++itbegin){
        scanpoint.insert(*itbegin);
    }
    for(auto itbegin = scan_from_90_to_135.begin(); itbegin != scan_from_90_to_135.end(); ++itbegin){
        scanpoint.insert(*itbegin);
    }
    for(auto itbegin = scan_from_135_to_180.begin(); itbegin != scan_from_135_to_180.end(); ++itbegin){
        scanpoint.insert(*itbegin);
    }

    for(auto itbegin = scanpoint.begin(); itbegin != scanpoint.end(); ++itbegin){
        for(const auto& robotpart : robotcenter){
            double distance = SqDistPointOBB(*std::get<0>(*itbegin),robotpart);
            minsqDistance = minsqDistance >  distance ? distance : minsqDistance;
        }
    }
    if(minsqDistance == 0){}
        //std::cout<<"Collision !\n";
    if(minsqDistance == 100000)
        minsqDistance = 16;
    m_distance = minsqDistance;
    //timer += time.getElapsedTimeInMilliSec();
    //std::cout<<"scan once consumes time = "<<timer<<"ms"<<std::endl;
    //std::cout<<"SCANPOINT.SIZE = "<<scanpoint.size()<<std::endl;
    //std::cout<<"minsqDistance = "<<minsqDistance<<std::endl;
    getOctreeNode(m_octree->GetRoot());
    scan_1.join();
    scan_2.join();
    scan_3.join();
    scan_4.join();
}
void  AXBOpenGLWidget::collider(){
    m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeOBB2D,getCollisionPoint);
    m_CollisionStrategy->calculateToOnePart(m_octree->GetRoot(),m_collider);
    double minsqDistance = 100000;
    for(auto itbegin = results.begin(); itbegin != results.end(); ++itbegin){
        double distance = SqDistPointOBB(**itbegin,m_collider);
        minsqDistance = minsqDistance >  distance ? distance : minsqDistance;
    }
    if(minsqDistance == 0){
        //std::cout<<"Collision !\n";
    }
    if(minsqDistance == 100000)
        minsqDistance = 4;
    m_distance = minsqDistance;
}
QMatrix4x4 AXBOpenGLWidget::setCamerView(viewmodel vm){
    QMatrix4x4 view;
    if(vm == ONE){
        float length = 4.f;
        float cameraPos_x = robotcenter[0].m_center.m_x-length*cos(m_rotation*PI/180);
        float cameraPos_y = robotcenter[0].m_center.m_y-length*sin(m_rotation*PI/180);
        float cameraPos_z = robotcenter[0].m_center.m_z+1;//+robotcenter[0].m_halfheight+1.0f;
        cameraPos = {cameraPos_x,cameraPos_y,cameraPos_z};
        float length2 = 4.f;
        float cameraTarget_x = robotcenter[0].m_center.m_x+length2*cos(m_rotation*PI/180);
        float cameraTarget_y = robotcenter[0].m_center.m_y+length2*sin(m_rotation*PI/180);
        float cameraTarget_z = robotcenter[0].m_center.m_z;//+robotcenter[0].m_halfheight+1.0f;

        //相机的目标点始终跟踪机器人
        cameraTarget = {cameraTarget_x,cameraTarget_y,cameraTarget_z};
        cameraTarget.setZ(-1.5);
        //视角设置   相机位置   相机目标点               辅助叉乘向量
        view.lookAt(cameraPos, cameraTarget,QVector3D(0.0, 0.0, 1.0));
    }
    else
        view.lookAt(cameraPos, cameraPos+cameraFront,QVector3D(0.0, 1.0, 0.0));
    return view;
}

void AXBOpenGLWidget::initViewThree(){
    yaw = -90;
    pitch = 0;
#ifndef POINTCLOUD
    cameraPos = QVector3D(robotcenter[0].m_center.m_x, robotcenter[0].m_center.m_y, VIEWHEIGHT);
#endif
#ifdef POINTCLOUD
    cameraPos = QVector3D(0, 0, VIEWHEIGHT);
#endif
    //Camera direction
    cameraTarget = QVector3D(robotcenter[0].m_center.m_x, robotcenter[0].m_center.m_y, robotcenter[0].m_center.m_z);
    cameraDirection = QVector3D(cameraPos - cameraTarget);
    cameraDirection.normalize();
    //Right axis
    up = QVector3D(0.0f, 1.0f, 0.0f);
    cameraRight = QVector3D::crossProduct(up, cameraDirection);
    cameraRight.normalize();
    //Up axis
    cameraUp = QVector3D::crossProduct(cameraDirection, cameraRight);
    cameraFront=QVector3D(0.0, 0.0, -1.0);
    camerForword = QVector3D(0.0, 1.0, 0.0);
}

void AXBOpenGLWidget::drawAxis(){
    //坐标变换，从局部坐标变换到世界坐标系中，主要就是 旋转 缩放 移动
    QMatrix4x4 model;
    glLineWidth(2);
    glBindVertexArray(VAO[2]);
    QVector4D color;
    model.setToIdentity();
    QVector3D tran {robotcenter[0].m_center.m_x,robotcenter[0].m_center.m_y,robotcenter[0].m_center.m_z};
    model.translate(tran);
    shaderProgram.setUniformValue("model", model);
    color = {255,0,0,1};
    shaderProgram.setUniformValue("color",color);
    glDrawArrays(GL_LINES,0,2);
    model.setToIdentity();
    model.translate(tran);
    model.rotate(-90,0,1,0);
    shaderProgram.setUniformValue("model", model);
    color = {0,0,255,0};
    shaderProgram.setUniformValue("color",color);
    glDrawArrays(GL_LINES,0,2);
    model.setToIdentity();
    model.translate(tran);
    model.rotate(90,0,0,1);
    shaderProgram.setUniformValue("model", model);
    color = {0,255,0,0};
    shaderProgram.setUniformValue("color",color);
    glDrawArrays(GL_LINES,0,2);
    glLineWidth(1);
}
void AXBOpenGLWidget::drawRay(){
    QMatrix4x4 model;
    glLineWidth(1);
    glBindVertexArray(VAO[2]);
    for(int angle = 0; angle < 360; angle+=45){
        QVector4D color;
        model.setToIdentity();
        QVector3D tran {lasers[0].m_center.m_x,lasers[0].m_center.m_y,lasers[0].m_center.m_z};
        model.translate(tran);
        model.rotate(angle,0,0,1);
        model.scale(3,1,1);
        shaderProgram.setUniformValue("model", model);
        color = {0,10,50,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_LINES,0,2);
    }
}

void AXBOpenGLWidget::drawRobot(){
    //坐标变换，从局部坐标变换到世界坐标系中，主要就是 旋转 缩放 移动
    QMatrix4x4 model;
    QVector4D robot;
    glBindVertexArray(VAO[0]);
    for(int i = 0; i < robotcenter.size(); ++i)
    {
        model.setToIdentity();
        //位移
        QVector3D trans {robotcenter[i].m_center.m_x,robotcenter[i].m_center.m_y,robotcenter[i].m_center.m_z};
        model.translate(trans);
        //旋转
        model.rotate(m_rotation,0,0,1);
        //缩放
        model.scale(robotcenter[i].m_halflength*2,robotcenter[i].m_halfwidth*2,robotcenter[i].m_halfheight*2);

        shaderProgram.setUniformValue("model",model);
        robot = {255.f,255.f,255.f,1.0f};
        shaderProgram.setUniformValue("color",robot);
        glDrawArrays(GL_TRIANGLES,0,36);
    }
}
void AXBOpenGLWidget::drawCollisionNode(){
    //坐标变换，从局部坐标变换到世界坐标系中，主要就是 旋转 缩放 移动
    QMatrix4x4 model;
    glBindVertexArray(VAO[0]);
    for(int i = 0; i < openglnode.size(); ++i){
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D t {openglnode[i]->m_x,openglnode[i]->m_y,openglnode[i]->m_z};
        model.translate(t);
        //缩放
        model.scale(0.05f,0.05f,0.05f);
        shaderProgram.setUniformValue("model", model);
        color = {255,255,0,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_TRIANGLES,0,36);
    }
}
void AXBOpenGLWidget::drawAllNode(){
    //坐标变换，从局部坐标变换到世界坐标系中，主要就是 旋转 缩放 移动
    QMatrix4x4 model;
    glPointSize(1);
    glBindVertexArray(VAO[1]);
    for (int i = 0; i < m_scenepoints.size(); ++i) {
        //标准化
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D t {m_scenepoints[i]->m_x,m_scenepoints[i]->m_y,m_scenepoints[i]->m_z};
        model.translate(t);
        //缩放
        //model.scale(0.01f,0.01f,0.01f);
        shaderProgram.setUniformValue("model", model);
        color = {255,255,255,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_POINTS,0,1);
    }
    glPointSize(2);
    for (int i = 0; i < m_positionpath.size(); ++i) {
        //标准化
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D t {m_positionpath[i]->m_x,m_positionpath[i]->m_y,m_positionpath[i]->m_z};
        model.translate(t);
        //缩放
        //model.scale(0.01f,0.01f,0.01f);
        shaderProgram.setUniformValue("model", model);
        color = {255,255,255,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_POINTS,0,1);
    }
    glBindVertexArray(VAO[0]);
    for (int i = 0; i < m_obstaclepoints.size(); ++i) {
        //标准化
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D t {m_obstaclepoints[i]->m_x,m_obstaclepoints[i]->m_y,m_obstaclepoints[i]->m_z};
        model.translate(t);
        //缩放
        model.scale(0.05f,0.05f,0.05f);
        shaderProgram.setUniformValue("model", model);
        color = {255,255,255,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_TRIANGLES,0,36);
    }
    glBindVertexArray(VAO[1]);
    for (int i = 0; i < m_noattpoints.size(); ++i) {
        //标准化
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D t {m_noattpoints[i]->m_x,m_noattpoints[i]->m_y,m_noattpoints[i]->m_z};
        model.translate(t);
        //缩放
        //model.scale(0.01f,0.01f,0.01f);
        shaderProgram.setUniformValue("model", model);
        color = {255,0,255,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_POINTS,0,1);
    }
    glPointSize(1);
}
//这个地方以为有可能不发生碰撞所以会存在nullptr点，crash掉
void AXBOpenGLWidget::drawRayPoint(){
    QMatrix4x4 model;
    glBindVertexArray(VAO[0]);

    for(auto itbegin = scanpoint.begin(); itbegin !=  scanpoint.end(); ++itbegin){
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D t {std::get<0>(*itbegin)->m_x,
                     std::get<0>(*itbegin)->m_y,
                     std::get<0>(*itbegin)->m_z};
        model.translate(t);
        //缩放
        model.scale(0.05f,0.05f,0.05f);
        shaderProgram.setUniformValue("model", model);
        color = {0,255,0,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_TRIANGLES,0,36);
    }
    for(int i = 0 ; i < waynode.size(); ++i){
        QVector4D color;
        model.setToIdentity();
        //位移
        QVector3D trans {waynode[i]->m_center.m_x,waynode[i]->m_center.m_y,waynode[i]->m_center.m_z};
        model.translate(trans);
        //缩放
        model.scale(waynode[i]->m_halflength*2,waynode[i]->m_halfwidth*2,waynode[i]->m_halfheight*2);

        shaderProgram.setUniformValue("model",model);
        color = {0,255,0,1};
        shaderProgram.setUniformValue("color",color);
        glDrawArrays(GL_TRIANGLES,0,36);
    }
}
void AXBOpenGLWidget::buildScene(){
    //+z面
    for(float back = -50,front = 50;back < front; back += 0.5){
        for(float left = -50,right = 50;left < right;left += 0.5){
            Point* scenepoint = new Point(left,back,1.5);
            OctreeNode::InsertObjectToScene(m_octree->GetRoot(),scenepoint);
            m_scenepoints.push_back(scenepoint);
        }
    }
    //+-y面
    for(float down = -1.5,beyond = 1.5;down < beyond; down += 0.5){
        for(float left = -50,right = 50;left < right;left += 0.5){
            Point* scenepoint_1 = new Point(left,-50,down);
            OctreeNode::InsertObjectToScene(m_octree->GetRoot(),scenepoint_1);
            m_scenepoints.push_back(scenepoint_1);
            Point* scenepoint_2 = new Point(left,50,down);
            OctreeNode::InsertObjectToScene(m_octree->GetRoot(),scenepoint_2);
            m_scenepoints.push_back(scenepoint_2);

        }
    }
    //+-x面
    for(float down = -1.5,beyond = 1.5;down < beyond; down += 0.5){
        for(float back = -50,front = 50;back < front;back += 0.5){
            Point* scenepoint_1 = new Point(-50,back,down);
            OctreeNode::InsertObjectToScene(m_octree->GetRoot(),scenepoint_1);
            m_scenepoints.push_back(scenepoint_1);
            Point* scenepoint_2 = new Point(50,back,down);
            OctreeNode::InsertObjectToScene(m_octree->GetRoot(),scenepoint_2);
            m_scenepoints.push_back(scenepoint_2);

        }
    }
}
void AXBOpenGLWidget::buildObject(const Point& center, const float& halflength,const float& halfwidth,const float& halfheight,const float& accuracy){

    float _back = center.m_y-halfwidth;
    float _front=center.m_y+halfwidth;
    float _beyond=center.m_z+halfheight;
    float _down =center.m_z-halfheight;
    float _left =center.m_x-halflength;
    float _right=center.m_x+halflength;
    //+z面
    for(float back = _back,front = _front;back < front; back += accuracy){
        for(float left = _left, right = _right;left < right;left += accuracy){
            Point* scenepoint = new Point(left,back,_beyond);
            m_octree->InsertObject(m_octree->GetRoot(),scenepoint);
            m_scenepoints.push_back(scenepoint);
        }
    }
    //+-y面
    for(float down = _down,beyond = _beyond;down < beyond; down += accuracy){
        for(float left = _left, right = _right;left < right;left += accuracy){
            Point* scenepoint_1 = new Point(left,_back,down);
            m_octree->InsertObject(m_octree->GetRoot(),scenepoint_1);
            m_scenepoints.push_back(scenepoint_1);
            Point* scenepoint_2 = new Point(left,_front,down);
            m_octree->InsertObject(m_octree->GetRoot(),scenepoint_2);
            m_scenepoints.push_back(scenepoint_2);
        }
    }
    //+-x面
    for(float down = _down,beyond = _beyond;down < beyond; down += accuracy){
        for(float back = _back,front = _front;back < front;back += accuracy){
            Point* scenepoint_1 = new Point(_left,back,down);
           m_octree->InsertObject(m_octree->GetRoot(),scenepoint_1);
            m_scenepoints.push_back(scenepoint_1);
            Point* scenepoint_2 = new Point(_right,back,down);
            m_octree->InsertObject(m_octree->GetRoot(),scenepoint_2);
            m_scenepoints.push_back(scenepoint_2);
        }
    }
}

void AXBOpenGLWidget::buildObstacle(const Point& center,const float& accuracy){
    float _back = center.m_y-0.5;
    float _front=center.m_y+0.5;
    float _beyond=center.m_z+0.5;
    float _down =center.m_z-0.5;
    float _left =center.m_x-0.5;
    float _right=center.m_x+0.5;
    //+z面
    for(float back = _back,front = _front;back < front; back += accuracy){
        for(float left = _left, right = _right;left < right;left += accuracy){
            Point* obstaclepoint = new Point(left,back,_beyond);
            OctreeNode::InsertObjectTonoattribute(m_octree->GetRoot(), obstaclepoint);
            m_noattpoints.push_back(obstaclepoint);
        }
    }
    //+-y面
    for(float down = _down,beyond = _beyond;down < beyond; down += accuracy){
        for(float left = _left, right = _right;left < right;left += accuracy){
            Point* obstaclepoint_1 = new Point(left,_back,down);
            OctreeNode::InsertObjectTonoattribute(m_octree->GetRoot(), obstaclepoint_1);
            m_noattpoints.push_back(obstaclepoint_1);
            Point* obstaclepoint_2 = new Point(left,_front,down);
            OctreeNode::InsertObjectTonoattribute(m_octree->GetRoot(), obstaclepoint_2);
            m_noattpoints.push_back(obstaclepoint_2);

        }
    }
    //+-x面
    for(float down = _down,beyond = _beyond;down < beyond; down += accuracy){
        for(float back = _back,front = _front;back < front;back += accuracy){
            Point* obstaclepoint_1 = new Point(_left,back,down);
            OctreeNode::InsertObjectTonoattribute(m_octree->GetRoot(), obstaclepoint_1);
            m_noattpoints.push_back(obstaclepoint_1);
            Point* obstaclepoint_2 = new Point(_right,back,down);
            OctreeNode::InsertObjectTonoattribute(m_octree->GetRoot(), obstaclepoint_2);
            m_noattpoints.push_back(obstaclepoint_2);
        }
    }
}
//获得所有光线相交的点
void AXBOpenGLWidget::getRayIntersectPoints(){
    scanpoint.clear();
    for(float yawangle = 0; yawangle < 360; yawangle += 90){
        for(float pitangle = 0; pitangle < 360; pitangle += 15){
            Point startpoint(lasers[0].m_center);
            Vector direction(cos(pitangle/180*PI)*cos(yawangle/180*PI),\
                             cos(pitangle/180*PI)*sin(yawangle/180*PI),\
                             sin(pitangle/180*PI));
            Ray ray(startpoint,direction);
            if(std::get<0>(getRayIntersectPoint(m_octree->GetRoot(),ray)) != nullptr)
            scanpoint.insert(getRayIntersectPoint(m_octree->GetRoot(),ray));
        }
    }
}
//获取贝塞尔曲线上的点
void AXBOpenGLWidget::getPath(const int& accuracy){
    m_positionpath.clear();
    float start = 0.f;
    float pre_x = 0.f,pre_y = 0.f,cur_x = 0.f,cur_y = 0.f;
    double target_length = beziercurve.total_length()/accuracy;
    for (int i = 0; i <= accuracy; ++i) {
        double t =beziercurve.getTByArcLength_Steffensen(start*i,target_length*(i+1));
        float dev_x = beziercurve.dev(t).m_x;
        float dev_y = beziercurve.dev(t).m_y;
        float newrotation = atanf(dev_y/dev_x);
        Point* pathPoint = new Point();
        *pathPoint = beziercurve.p0 * (pow(1 - t, 3))
      + beziercurve.p1 * 3 * t * pow(1 - t,2)
      + beziercurve.p2 * 3 * pow(t, 2) * (1 - t)
      + beziercurve.p3 * pow(t, 3);
        cur_x = beziercurve.at(t).m_x;
        cur_y = beziercurve.at(t).m_y;
        if(GT(cur_x,pre_x)){
        }
        else if(LT(cur_x,pre_x)){
            newrotation += PI;
        }
        else if(EQ(cur_x,pre_x) && GT(cur_y,pre_y)){
            newrotation = PI/2;
        }
        else if(EQ(cur_x,pre_x) && LT(cur_y,pre_y)){
            newrotation = -PI/2;
        }
        pre_x = cur_x;
        pre_y = cur_y;
        m_positionpath.push_back(pathPoint);
        m_rotationpath.push_back(newrotation);
    }
}
void AXBOpenGLWidget::setRobotPos(const Point& newcenter,const float& newrotation){
    m_rotation = newrotation/PI*180;
    robotcenter[0].m_center.m_x = newcenter.m_x;
    robotcenter[0].m_center.m_y = newcenter.m_y;
    robotcenter[0].setAxe(newrotation);
    robotcenter[3].setAxe(newrotation);
    robotcenter[1].setAxe(newrotation);
    robotcenter[2].setAxe(newrotation);
    robotcenter[0].setVert(newrotation);
    robotcenter[3].setVert(newrotation);
    robotcenter[1].setVert(newrotation);
    robotcenter[2].setVert(newrotation);
    robotcenter[1].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(m_atan+newrotation);
    robotcenter[1].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(m_atan+newrotation);
    robotcenter[2].m_center.m_x = robotcenter[0].m_center.m_x + m_len*cos(-m_atan+newrotation);
    robotcenter[2].m_center.m_y = robotcenter[0].m_center.m_y + m_len*sin(-m_atan+newrotation);
    robotcenter[3].m_center = robotcenter[0].m_center + Vector(0,0,0.75);
    lasers[0].m_center.m_y = (robotcenter[1].m_center.m_y+robotcenter[2].m_center.m_y)/2;
    lasers[0].m_center.m_x = (robotcenter[1].m_center.m_x+robotcenter[2].m_center.m_x)/2;
}
bool AXBOpenGLWidget::noCollisionPath(){
    CELLTimestamp time;
    float timer = 0.0;
    for(int i = 0; i < 1000 && m_pos < (m_bzaccuracy-2); ++i){
//            ++m_pos;
//            setRobotPos(*m_positionpath[m_pos],m_rotationpath[m_pos]);
//            std::vector<Cuboid> robot_pos_1 = {};
//            for(int j = 0; j < 4; ++j){
//                robot_pos_1.push_back(robotcenter[j]);
//            }
        time.update();
        ++m_pos;
        setRobotPos(*m_positionpath[m_pos],m_rotationpath[m_pos]);
        std::vector<Cuboid> robot_pos_2 = {};
        for(int j = 0; j < 4; ++j){
            //robot_pos_2.push_back(buildOBB(robot_pos_1[j],robotcenter[j]));
            robot_pos_2.push_back(robotcenter[j]);
        }

//            std::cout<<"robot_pos_2->M_HALFLENGTH = "<<robot_pos_2[0].m_halflength<<std::endl;
//            std::cout<<"robot_pos_2->M_HALFWIDTH  = "<<robot_pos_2[0].m_halfwidth<<std::endl;
//            std::cout<<"robot_pos_2->center_x = "<<robot_pos_2[0].m_center.m_x<<std::endl;
//            std::cout<<"robot_pos_2->center_y = "<<robot_pos_2[0].m_center.m_y<<std::endl;

        ++m_pos;
        setRobotPos(*m_positionpath[m_pos],m_rotationpath[m_pos]);
        std::vector<Cuboid> robot_OBB = {};
        for(int j = 0; j <4; ++j){
           robot_OBB.push_back(buildOBB(robot_pos_2[j],robotcenter[j]));
        }

//            std::cout<<"OBB->M_HALFLENGTH = "<<robot_OBB[0].m_halflength<<std::endl;
//            std::cout<<"OBB->M_HALFWIDTH  = "<<robot_OBB[0].m_halfwidth<<std::endl;
//            std::cout<<"OBB->center_x = "<<robot_OBB[0].m_center.m_x<<std::endl;
//            std::cout<<"OBB->center_y = "<<robot_OBB[0].m_center.m_y<<std::endl;

        m_CollisionStrategy = new CollisionStrategy(getCollisionOctreeNodeOBB2D,getCollisionPoint);
        if(m_CollisionStrategy->calculateToParts(m_octree->GetRoot(),robot_OBB)){
            std::cout<<"Collision!\n";
            m_pos = 0;
            std::cout<<"Total consumes time = "<<timer<<"ms" << std::endl;
            return true;
        }
        //m_CollisionStrategy->clearResult();
        timer+=time.getElapsedTimeInMilliSec();
    }



    timer = 0;
    return false;
}
